package edu.asu.sapa.heuristic;


import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;

import edu.asu.sapa.Planner;
import edu.asu.sapa.ground.Operator;
import edu.asu.sapa.ground.PropDB;
import edu.asu.sapa.ground.Proposition;
import edu.asu.sapa.ground.State;
import edu.asu.sapa.ground.UpdateQ;
import edu.asu.sapa.ground.update.Context;
import edu.asu.sapa.ground.update.UpdateWrapper;
import edu.asu.sapa.utils.Utility;

public final class PG {

	public final static int CostPropMax=0;
	public final static int CostPropSum=1;
	public final static int CostPropCombo=2;

	public int costPropOption = CostPropMax;
	public int lookaheadOption = 1;
	public boolean pspFlag = false;

	// ArrayList<Operator> operators;
	// ArrayList<Proposition> propositions;
	public State currentState;
	// FluentDB mresDB;

	public ArrayList<Operator> operators;
	public int numActions;

	//	private int[] actionCostID;
	//	private float[] maxActionCost;
	//	private float[] sumActionCost;
	//	private float[] zeros;

	public ArrayList<Proposition> propositions;
	public int numFacts;

	public int remainingHardGoals;

	public Proposition[] pendingPropositions=new Proposition[1000];
	public float[] pendingPropositionTimes=new float[1000];
	public int pendingPropositionsSize;
	public Operator[] pendingOperators=new Operator[1000];
	public float[] pendingOperatorTimes=new float[1000];
	public int pendingOperatorsSize;

	public static class pgGoal {
		public int id;
		public float time;
		public float reward;
		public float penalty;
		public pgGoal(int id, float time, float reward, float penalty) {
			this.id = id;
			this.time = time;
			this.reward = reward;
			this.penalty = penalty;
		}
		public void set(int id, float time, float reward, float penalty) {
			this.id = id;
			this.time = time;
			this.reward = reward;
			this.penalty = penalty;
		}
		public pgGoal(int id, float time) {
			this.id = id;
			this.time = time;
			this.reward = 0;
			this.penalty = Float.POSITIVE_INFINITY;
		}
		public pgGoal() {
			// TODO Auto-generated constructor stub
		}
		public void set(int id, float time) {
			this.id = id;
			this.time = time;
			this.reward = 0;
			this.penalty = Float.POSITIVE_INFINITY;
		}
	}

	// treat like a stack
	public pgGoal[] goals = new pgGoal[500];
	public int goalsSize;

	private final pgGoal newpgGoal() {
		if (goalsSize >= goals.length) {
			goals = Utility.grow(goals);
		}
		pgGoal ret = goals[goalsSize];
		if (ret == null) {
			ret = new pgGoal();
			goals[goalsSize] = ret;
		}
		++goalsSize;
		return ret;
	}

	public int selectedOperator;
	public int h;
	public float hCost;
	public float hReward;

	public Operator[] potentialOperators;
	public int nPotentialOperators;
	public Operator[] helpfulActions;
	public int nHelpfulActions;


	public PG() {
	}

	// rp actions are already helpful actions
	public Operator[] helpfulActions() {
		Operator act;
		int id;

		nHelpfulActions=0;
		search:	for (int i = 0; i < nPotentialOperators; ++i) {
			act = potentialOperators[i];
			//act = operators.get(id);

			if (act.selected()) {
				helpfulActions[nHelpfulActions++] = act;
				continue search;
			}

			for (int eID : act.adds) {
				if (propositions.get(eID).selected()) {
					helpfulActions[nHelpfulActions++] = act;
					continue search;
				}
			}
		}
		// for (int i = 0; i < nPotentialActions; ++i) {
		// id = potentialActions[i];
		// if (readyFlag[id]) {
		// helpfulActions[nHelpfulActions++] = id;
		// }}
		Operator[] a = new Operator[nHelpfulActions];
		System.arraycopy(helpfulActions, 0, a, 0, nHelpfulActions);
		return a;
	}

	/**
	 * @return ids of actions in the relaxed plan that are applicable right now
	 */
	public Operator[] rpActions() {
		Operator act;
		int id;

		nHelpfulActions=0;
		for (int i = 0; i < nPotentialOperators; ++i) {
			act = potentialOperators[i];
			//act = operators.get(id);

			if (act.selected()) {
				helpfulActions[nHelpfulActions++] = act;
				continue;
			}
		}
		Operator[] a = new Operator[nHelpfulActions];
		System.arraycopy(helpfulActions, 0, a, 0, nHelpfulActions);
		return a;
	}

	public Operator[] applicableOperators() {
		Operator[] a = new Operator[nPotentialOperators];
		System.arraycopy(potentialOperators, 0, a, 0, nPotentialOperators);
		return a;
	}

	/***************************************************************************
	 * Set of functions to build the bi-level graph structure to help calculate
	 * the relaxed plan fast.
	 **************************************************************************/

	public void buildBiLevelGraph(ArrayList<Operator> operators, ArrayList<Proposition> propositions) {
		this.operators = operators;
		int n = numActions;
		numActions = operators.size();
		if (numActions > n) {
			//			actionCostID = new int[numActions];
			//			maxActionCost = new float[numActions];
			//			sumActionCost = new float[numActions];
			//			zeros = new float[numActions];
			potentialOperators = new Operator[numActions];
			helpfulActions = new Operator[numActions];
		}

		this.propositions = propositions;
		numFacts = propositions.size();
		
		costs = new float[numFacts];
	}


	public boolean costPropagation(State currentState) {
		int i;
		int size;

		if (!currentState.constraintsHoldAt())
			return false;

		this.currentState = currentState;
		float time = currentState.time;

		reset(time);

		PropDB db = currentState.propDB;
		PropDB.Entry[] dbTable = db.table;
		PropDB.Entry entry;

		size = dbTable.length;
		Operator o;
		Proposition p;
		float epsTime = time + Planner.EPSILON;

		boolean before, after;
		for(i=0; i<size; ++i) {
			entry = dbTable[i];
			if (entry == null)
				continue;
			before = entry.getValueBefore(time,true);
			after = entry.getValueAfter(true);
			if (before && after) {
				p = propositions.get(entry.key);
				p.addRelaxedCost(time,0,0);
				addPendingPropositionInternal(p,time);
			}
			// for non-PDDL problems, can't use the && after bit, since actions might only require something in ...)
			// PDDL actions that only require in ...) always simul. change that thing, and that will be mutex if before && !after holds
		}
		
		propagatePropositions();
		
		i=0;
		while (i < pendingOperatorsSize) {
			o = pendingOperators[i];
			if (!isPotentialOperator(o,time)) {
				o.times[0] = epsTime;
				pendingOperatorTimes[i] = epsTime;
				++i;
			} else {
				activateOperator(o,time);
				--pendingOperatorsSize;
				pendingOperators[i] = pendingOperators[pendingOperatorsSize];
				pendingOperatorTimes[i] = pendingOperatorTimes[pendingOperatorsSize];
			}
		}

		for(i=0; i<size; ++i) {
			entry = dbTable[i];
			if (entry == null)
				continue;
			before = entry.getValueBefore(time,true);
			after = entry.getValueAfter(true);
			if (!before && after) {
				p = propositions.get(entry.key);
				if (p.addRelaxedCost(epsTime,0,0))
					addPendingPropositionInternal(p,epsTime);
			}
		}

		UpdateQ<?> q = currentState.events;
		UpdateWrapper<?>[] table;
		UpdateWrapper<?> u;

		table = q.table;
		size = q.size;
		for (i=0; i < size; ++i) {
			u = table[i];
			u.relaxedUpdate(this,currentState);
		}
		
		int l=0;
		
		if (lookaheadOption < 0) {
			while (pendingPropositionsSize > 0) {
				propagatePropositions();
				propagateOperators();
				++l;
				if (l > 20)
					System.err.println("Too many plan graph levels for ReplanPfile10.");
			}
			if (this.remainingHardGoals>0)
				return false;
		} else {
			while (this.remainingHardGoals > 0) {
				if (pendingPropositionsSize <= 0)
					return false;
				propagatePropositions();
				propagateOperators();
			}

			for (i = lookaheadOption; i > 0 && pendingPropositionsSize > 0; --i) {
				propagatePropositions();
				propagateOperators();
			}
		}
		return true;
	}

	public Context applicableContext = new Context();

	private final void setPotentialOperators(float time) {
		Operator o;
		applicableContext.start = time;
		float dur;
		for (int i=0; i < pendingOperatorsSize; ++i) {
			o = pendingOperators[i];
			applicableContext.id  = o.id;
			applicableContext.duration = dur = o.duration.value(currentState);
			applicableContext.finish = time + dur;
			if (o.applicable(currentState,applicableContext))
				potentialOperators[nPotentialOperators++] = o;
			else
				pendingOperatorTimes[i] += Planner.EPSILON;
		}
	}
	private final boolean isPotentialOperator(Operator o, float time) {
		applicableContext.start = time;
		float dur;
		applicableContext.id  = o.id;
		applicableContext.duration = dur = o.duration.value(currentState);
		applicableContext.finish = time + dur;
		if (o.applicable(currentState,applicableContext)) {
			potentialOperators[nPotentialOperators++] = o;
			return true;
		}
		return false;
	}

	private final void activateOperator(Operator o, float time) {
		int index = o.getIndex(time);
		int support = o.supportIDs[index];
		if (support > 0)
			return;
		o.supportIDs[index] = -support;
		float cost = o.costs[index];
		if (cost >= Float.POSITIVE_INFINITY)
			System.err.println("AckBar!!");
		float finish = time + o.duration.value(currentState) + Planner.EPSILON;
		float c = o.cost.value(currentState);
		if (c >= Float.POSITIVE_INFINITY) 
			System.err.println("AckBarBaz!!");
		float effectCost = cost + o.cost.value(currentState);
		if (effectCost >= Float.POSITIVE_INFINITY)
			System.err.println("Ack!!");
		int id = o.id;
		for (int i=0; i < o.addsSize; ++i) {
			int pID = o.adds[i];
			Proposition p = propositions.get(pID);
			if (p.addRelaxedCost(finish,effectCost,-id))
				addPendingPropositionInternal(p,finish);
		}
	}

	private final void activateProposition(Proposition p, float time) {
		float cost;
		int id = p.id;
		if (p.isHard) {
			--remainingHardGoals;
			p.isHard = false;
		}
		assert time < Float.POSITIVE_INFINITY;
		int index = p.getIndex(time);
		int support = p.supportIDs[index];
		if (support > 0)
			return;
		p.supportIDs[index] = -support;
		if (p.size == 1) {
			for (int i = 0; i < p.conditionsSize; ++i) {
				int spActID = p.conditions[i];
				Operator o = operators.get(spActID);
				// o.size > 0
				// this means the action is currently applicable, however, a precondition is presently being clobbered.
				// figure out how to deal with these things.
				// perhaps process the effects directly but don't add the operator?
				if (++o.nConditionsSat == o.conditionsSize) {
					initializeDeltas(o);
					switch(costPropOption) {
					case CostPropMax:
						initializeRelaxedCostMax(o);
						break;
					case CostPropSum:
						initializeRelaxedCostSum(o);
						break;
					case CostPropCombo:
						initializeRelaxedCostCombo(o);
						break;
					}
				}
			}
		} else {
			for (int i = 0; i < p.conditionsSize; ++i) {
				int spActID = p.conditions[i];
				Operator o = operators.get(spActID);
				if (o.nConditionsSat == o.conditionsSize) {
					index = o.getIndex(time);
					if (index < 0)
						index = 0;
					for (int j=0; j < o.conditionsSize;++j) {
						if (o.times[0] < propositions.get(o.conditions[j]).times[0])
							System.err.println("BlearghRargh");
					}
					for (int j=index; j < o.size; ++j) {
						float t = o.times[j];
						cost = getActionCost(o,t);
						if (o.addRelaxedCost(t,cost,-id))
							addPendingOperatorInternal(o,t);
					}
//					cost = this.getActionCost(spActID,time);
//					// need the infinity check because things can happen out of order
//					if (cost < Float.POSITIVE_INFINITY && o.addRelaxedCost(time,cost,-id)) {
//						addPendingOperatorInternal(o,time);
//					}
				}
			}

		}
	}

	private float getActionTime(int aID) {
		Operator act = operators.get(aID);
		int id;
		Proposition f;
		float t,m=Float.NEGATIVE_INFINITY;
		for (int i = act.conditionsSize - 1; i >= 0; --i) {
			id = act.conditions[i];
			f = propositions.get(id);
			if (f.size > 0) {
				t = f.times[f.size - 1];
				if (t > m) {
					m = t;
				}
			}
		}
		return m;
	}

	private float getActionCost(Operator act, float time) {
		int id;
		Proposition f;
		float t,m=Float.NEGATIVE_INFINITY;
		float s=0;
		for (int i = act.conditionsSize - 1; i >= 0; --i) {
			id = act.conditions[i];
			f = propositions.get(id);
			t=f.getRelaxedCost(time);
			s+=t;
			if (t > m) {
				m = t;
			}
		}
		switch(costPropOption) {
		case CostPropMax:
			return m;
		case CostPropSum:
			return s;
		case CostPropCombo:
			return (m + s ) / 2.0f;
		}
		return m;
	}
	
	final void initializeDeltas(Operator o) {
		int i, id=-1;
		int s = o.conditionsSize;
		int t;
		Proposition p;
		int[] conditions = o.conditions;
		float[] pCosts;
		float[] pTimes;
		float cost;
		float delta;
		if (o.size > 0)
			o.size = 0;
		for(i=0;i<s;++i) {
			id = conditions[i];
			p = propositions.get(id);
			pCosts = p.costs;
			pTimes = p.times;
			t = p.size;
			o.addRelaxedCostDelta(pTimes[0],cost=pCosts[0],id);
			if (pCosts[0] >= Float.POSITIVE_INFINITY)
				System.err.println("Baz");
			for(int j=1; j<t;++j) {
				// skip an index if support at that index is nonpositive?
				// when that is the case there are pending events that will lower the cost profile anyways (perhaps more efficiently; perhaps less)
				delta = pCosts[j] - cost;
				o.addRelaxedCostDelta(pTimes[j],delta,id);
				cost += delta;
				if (delta >= Float.POSITIVE_INFINITY)
					System.err.println("Baz");
			}
		}
	}
	
	float [] costs;
	public final void initializeRelaxedCostSum(Operator o) {
		int i;
		int id=-1;
		int k;
		int s = o.conditionsSize;
		int t = o.size;
		float[] costs = this.costs;
		int[] conditions = o.conditions;
		float[] deltas = o.costs;
		float[] times = o.times;
		int[] supports = o.supportIDs;
		float sum=0;
		float delta;
		float cost;
		float time=Float.NEGATIVE_INFINITY;
		float next;
		for(i=0; i < s; ++i) {
			costs[conditions[i]] = Float.POSITIVE_INFINITY;
		}
		k=s;
		i=0;
		do {
			id = supports[i];
			delta = deltas[i];
			sum += delta;
			cost = costs[id];
			if (cost >= Float.POSITIVE_INFINITY) {
				--k;
				costs[id] = delta;
				if (k == 0)
					break;
			} else {
				costs[id] = cost + delta;
			}
			++i;
		} while(true);
		time = times[i];
		assert time >= 0;
		times[k] = time;
		deltas[k] = sum*0.5f;
		supports[k] = -id;
		addPendingOperatorInternal(o,time);
		for(++i; i < t; ++i) {
			id = supports[i];
			next = times[i];
			sum += deltas[i];
			if (next > time) {
				++k;
				times[k] = next;
				deltas[k] = sum;
				supports[k] = -id;
				time=next;
				addPendingOperatorInternal(o,time);
			} else {
				deltas[k] = sum;
			}
		}
		o.size = k+1;
	}
	public final void initializeRelaxedCostMax(Operator o) {
		int i, j;
		int id;
		int k;
		int s = o.conditionsSize;
		int t = o.size;
		float[] costs = this.costs;
		int[] conditions = o.conditions;
		float[] deltas = o.costs;
		float[] times = o.times;
		int[] supports = o.supportIDs;
		float max=0;
		int mID=-1;
		float delta;
		float cost;
		float time=Float.NEGATIVE_INFINITY;
		float next;
		for(i=0; i < s; ++i) {
			costs[conditions[i]] = Float.POSITIVE_INFINITY;
		}
		k=s;
		i=0;
		do { 
			id = supports[i];
			delta = deltas[i];
			cost = costs[id];
			if (cost >= Float.POSITIVE_INFINITY) {
				--k;
				costs[id] = delta;
				if (delta > max) {
					max = delta;
					mID=id;
				}
				if (k==0)
					break;
			} else {
				costs[id] = cost + delta;
			}
			++i;
		} while(true);
		assert k == 0;
		time = times[i];
		assert time >= 0;
		times[k] = time;
		deltas[k] = max;
		supports[k] = -mID;
		addPendingOperatorInternal(o,time);
		for(++i; i < t; ++i) {
			id = supports[i];
			next = times[i];
			if (id != mID) {
				costs[id] += deltas[i];
			} else {
				delta = deltas[i];
				costs[id] += delta;
				max += delta;
				for(j=0; j < s; ++j) {
					id = conditions[j];
					cost = costs[id];
					if (cost > max) {
						mID = id;
						max = cost;
					}
				}
				if (next > time) {
					++k;
					times[k] = next;
					deltas[k] = max;
					supports[k] = -mID;
					time=next;
					addPendingOperatorInternal(o,time);
				} else {
					deltas[k] = max;
					supports[k] = -mID;
				}
			}
		}
		o.size = k+1;
	}

	public final void initializeRelaxedCostCombo(Operator o) {
		int i, j;
		int k;
		int id;
		int s = o.conditionsSize;
		int t = o.size;
		float[] costs = this.costs;
		int[] conditions = o.conditions;
		float[] deltas = o.costs;
		float[] times = o.times;
		int[] supports = o.supportIDs;
		float sum=0;
		float max=Float.NEGATIVE_INFINITY;
		int mID=-1;
		float delta;
		float cost;
		float time=Float.NEGATIVE_INFINITY;
		float next;
		for(i=0; i < s; ++i) {
			costs[conditions[i]] = Float.POSITIVE_INFINITY;
		}
		k=s;
		i=0;
		do {
			id = supports[i];
			delta = deltas[i];
			sum += delta;
			cost = costs[id];
			if (cost >= Float.POSITIVE_INFINITY) {
				--k;
				costs[id] = delta;
				if (delta > Float.POSITIVE_INFINITY)
					System.err.println("Woof");
				if (delta > max) {
					max = delta;
					mID = id;
				}
				if (k == 0)
					break;
			} else {
				costs[id] = cost + delta;
			}
			++i;
		} while(true);
		if (k != 0)
			System.err.println("Bar");
		if (sum >= Float.POSITIVE_INFINITY || max >= Float.POSITIVE_INFINITY)
			System.err.println("FooBar");
		assert k == 0;
		time = times[i];
		assert time >= 0;
		if (t >= 2) {
			if (times[0] > times[1])
				System.err.println("Foo");
		}
		times[k] = time;
		deltas[k] = (max + sum)*0.5f;
		supports[k] = -mID;
		addPendingOperatorInternal(o,time);
		for(++i; i < t; ++i) {
			id = supports[i];
			next = times[i];
			delta = deltas[i];
			if (delta >= Float.POSITIVE_INFINITY)
				System.err.println("Tark!");
			cost = costs[id];
			if (cost >= Float.POSITIVE_INFINITY) {
				System.err.println("Bark!");
			}
			costs[id] = cost + delta;
			sum += delta;
			if (id == mID) {
				if (delta > 0)
					System.err.println("Blargh!");
				max += delta;
				for(j=0; j < s; ++j) {
					id = conditions[j];
					cost = costs[id];
					if (cost >= Float.POSITIVE_INFINITY)
						System.err.println("Meow!");
					if (cost > max) {
						mID = id;
						max = cost;
					}
				}
			}
			if (sum >= Float.POSITIVE_INFINITY || max >= Float.POSITIVE_INFINITY)
				System.err.println("Blearggghghgh!");
			if (next > time) {
				++k;
				times[k] = next;
				deltas[k] = (max + sum)*0.5f;
				supports[k] = -mID;
				time=next;
				addPendingOperatorInternal(o,time);
			} else {
				deltas[k] = (max + sum)*0.5f;
				supports[k] = -mID;
			}
		}
		o.size = k+1;
	}
	
	public final void addPendingOperator(Operator o, float time, float cost, int supportID) {
		if (o.addRelaxedCost(time,cost,supportID))
			addPendingOperatorInternal(o,time);
	}

	public final void addPendingOperatorInternal(Operator o, float time) {
		if (pendingOperatorsSize >= pendingOperators.length) {
			pendingOperators = Utility.grow(pendingOperators);
			pendingOperatorTimes = Utility.grow(pendingOperatorTimes);
		}
		pendingOperators[pendingOperatorsSize] = o;
		pendingOperatorTimes[pendingOperatorsSize] = time;
		++pendingOperatorsSize;
	}

	// consider using exceptions to handle array size increases for PG
	// since the arrays will eventually get big enough to no longer cause exceptions.
	// can even use a pre-generated exception object to save some effort
	private final void addPendingPropositionInternal(Proposition p, float time) {
		if (pendingPropositionsSize >= pendingPropositions.length) {
			pendingPropositions = Utility.grow(pendingPropositions);
			pendingPropositionTimes = Utility.grow(pendingPropositionTimes);
		}
		pendingPropositions[pendingPropositionsSize] = p;
		pendingPropositionTimes[pendingPropositionsSize] = time;
		++pendingPropositionsSize;
	}
	public final void addPendingProposition(Proposition p, float time, float cost, int supportID) {
		if (p.addRelaxedCost(time,cost,supportID))
			addPendingPropositionInternal(p,time);
	}

	/**
	 * TODO, FIXME
	 * 
	 * @return
	 */
	public HashSet<Integer> getSelectedGoals() {
		// if (relaxedPlanOption) {
		// relaxedPlanHeuristic();
		// }
		// (if no RP extraction, then should be all hard goals
		// but this is untested)
		return null;
	}


	private final void propagateOperators() {
		Operator[] a = pendingOperators;
		float[] b = pendingOperatorTimes;
		int size = pendingOperatorsSize;
		for (int index = 0; index < size; ++index) {
			activateOperator(a[index],b[index]);
		}
		pendingOperatorsSize = 0;
	}

	private final void propagatePropositions() {
		Proposition[] a = pendingPropositions;
		float[] b = pendingPropositionTimes;
		int size = pendingPropositionsSize;
		for (int index = 0; index < size; ++index) {
			activateProposition(a[index],b[index]);
		}
		pendingPropositionsSize = 0;
	}


	public final float getHCost() {
		return hCost;
	}

	int[] rp = new int[500];
	float[] rpTime = new float[500];
	int rpSize;
	
	
	public final int relaxedPlanHeuristic() {
		int aID;
		float aTime;
		int gID;
		float gTime;
		Operator act;
		pgGoal goal;

		int gIndex = -1;
// queue		
		for (gIndex=0; gIndex < goalsSize; ++gIndex) {
			goal = goals[gIndex];
// stack
//		while(goalsSize > 0) {
//			goal = goals[--goalsSize];
		
			gID = goal.id;
			gTime = goal.time;
			Proposition g = propositions.get(gID);
			if (g.selected(gTime))
				continue;

			// compare 	g.getCost(gTime) - goal.reward 
			// and 		goal.penalty
			// for the ultimate in awesomeness, propagate delete reachability
			// perhaps by using (Literal extends CostFunction) Proposition.trueLiteral, Proposition.falseLiteral; 

			//if (g.getRelaxedCost(gTime) - goal.reward >= goal.penalty) {  // doesn't work except, maybe, for max propagation
			if (!g.achievable(gTime)) {
				hCost += goal.penalty;
//				System.err.println("Unachievable Goal: " + g.id);
				continue;
			}
			
			hReward += goal.reward;
			g.select(gTime);

			gTime = g.getMinTime(gTime);
			aID = g.getSupport(gTime);

			if (aID == 0)
				continue; // hCost += g.getRelaxedCost(gTime) ?  (normally 0...)

			// since the goal wasn't selected, the supporting action is also unselected (see below)

			act = operators.get(aID);
			float dur = act.duration.value(currentState);
			aTime = gTime - dur - Planner.EPSILON;
			aTime = act.getMinTime(aTime);

			//selectedOperator = act.id;
			if (act.selected(aTime)) {
				System.err.println("Actions shouldn't be selectable twice...");
				continue;
			}
			act.select(aTime);
			h += 1;
			float cost = act.cost.value(currentState);
			hCost += cost;
			
			if (cost >= Float.POSITIVE_INFINITY) {
				System.err.println("Infinite cost action should be impossible (in any problem?).");
			}
			
			rpInsert(aID,aTime);
			
			if (rpSize > 50) {
				System.err.println("Relaxed plan too big for ReplanPfile10, ZenoTravel.");
			}
			
			// (from above) because all effects of selected actions are selected
			for (int i=0; i < act.addsSize; ++i) {
				int id = act.adds[i];
				propositions.get(id).select(gTime);
			}

			cost = act.getRelaxedCost(aTime); 
			if (cost <= 0)
				continue;
			
			if (cost >= Float.POSITIVE_INFINITY) {
				System.err.println("Infinite propagated cost, or RP extraction went too far backward in time.");
				continue;
			}

			if (goalsSize + act.conditionsSize > goals.length) {
				pgGoal[] old = goals;
				int s = goalsSize - (++gIndex);
				goals = new pgGoal[s + act.conditionsSize + (goals.length>>1)];
				System.arraycopy(old,gIndex,goals,0,s);
				goalsSize=s;
				gIndex=-1;
			}
			for(int i=0; i < act.conditionsSize; ++i) {
				int id = act.conditions[i];
				g = propositions.get(id);
				if (g.selected(aTime))
					continue;
				addGoalInternal(id,aTime);
			}

		}

		return h;
	}

	private final void rpInsert(int aID, float time) {
		if (rpSize >= rp.length) {
			rp = Utility.grow(rp);
			rpTime = Utility.grow(rpTime,rp.length);
		}
		int i;
		for(i=rpSize-1; i>=0; --i) {
			if (rpTime[i] >= time) {
				break;
			}
		}
		++i;
		Utility.insert(rp,i,aID,rpSize);
		Utility.insert(rpTime,i,time,rpSize);
		rpSize++;
	}

	public final void addGoal(int id, float time) {
		pgGoal goal;
		if (goalsSize >= goals.length) {
			goals = Utility.grow(goals);
			goals[goalsSize++] = new pgGoal(id,time);
		} else {
			goal = goals[goalsSize];
			if (goal == null) {
				goal = new pgGoal();
				goals[goalsSize] = goal;
			}
			goal.set(id,time);
			++goalsSize;
		}
		Proposition p = propositions.get(id);
		if (p.size == 0) {
			p.isHard = true;
			++remainingHardGoals;
		}
	}

	public final void addGoal(int id, float time, float reward, float penalty) {
		if (goalsSize >= goals.length) {
			goals = Utility.grow(goals);
			goals[goalsSize++] = new pgGoal(id,time, reward, penalty);
		} else {
			pgGoal goal;
			goal = goals[goalsSize];
			if (goal == null) {
				goal = new pgGoal();
				goals[goalsSize] = goal;
			}
			goal.set(id,time,reward,penalty);
			++goalsSize;
		}
		Proposition p = propositions.get(id);
		if (penalty >= Float.POSITIVE_INFINITY && p.size == 0) {
			p.isHard = true;
			++remainingHardGoals;
		}
	}

	private final void addGoalInternal(int id, float time) {
		pgGoal goal;
		goal = goals[goalsSize];
		if (goal == null) {
			goal = new pgGoal();
			goals[goalsSize] = goal;
		}
		goal.set(id,time);
		++goalsSize;
	}

	private final void reset(float time) {
		int i;
		//		System.arraycopy(zeros, 0, maxActionCost, 0, numActions);
		//		System.arraycopy(zeros, 0, sumActionCost, 0, numActions);
		pendingOperatorsSize = 0;
		pendingPropositionsSize = 0;

		nPotentialOperators = 0;

		hReward = 0f;
		hCost = 0f;
		h=0;
		selectedOperator=0;

		Operator o;
		for (i = 1; i < numActions; i++) {
			o = operators.get(i);
			o.deactivate();
			if (o.conditionsSize == 0 && time < Float.POSITIVE_INFINITY) {
				o.addRelaxedCost(time,0,0);
				addPendingOperatorInternal(o,time);
			}
		}
		Proposition p;
		for (i = 0; i < numFacts; i++) {
			p = propositions.get(i);
			p.deactivate();
		}

		remainingHardGoals=0;
		
		rpSize=0;
		goalsSize = 0;
	}

	public void resetHAOption(boolean haf, boolean hanef) {
	}

	public void resetLAOption(int lao) {
		lookaheadOption = lao;
	}


	public void optionSetting(Planner p) {
		// deadlineFlag = dlf;
		costPropOption = p.costPropOption;
		lookaheadOption = p.lookaheadOption;
		pspFlag = true;
	}

	public int[] getRelaxedPlan() {
		int[] a = new int[rpSize];
		System.arraycopy(rp,0,a,0,rpSize);
		return a;
	}

	// TODO 
	// FIXME
	public void setSelectedGoals(HashSet<Integer> goalSet) {

	}
}
